
#include <SimpleFOC.h>


// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(PB6,PB7,PB8,PB9);

// encoder instance
Encoder encoder = Encoder(PB0, PB1, 1000);

MagneticSensorSPI ma730 = MagneticSensorSPI(MA730_SPI,PA4);
LowPassFilter filter = LowPassFilter(0.005);
LowPassFilter base_filter = LowPassFilter(0.005);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}

// velocity set point variable
float target_force = 0;
//这里是校准齿槽效应的数据，每个电机不一样，校准后电机更顺滑，不校准问题也不大
//float calib[1001]={-0.02000,-0.03600,-0.05000,-0.07000,-0.08600,-0.08800,-0.09600,-0.10800,-0.12200,-0.12600,-0.12000,-0.09600,-0.05600,-0.00200,0.03600,0.07400,0.09200,0.09600,0.09400,0.09400,0.09200,0.09600,0.10400,0.11600,0.13400,0.15400,0.17200,0.18600,0.18600,0.18000,0.15800,0.13800,0.11600,0.10200,0.09000,0.08800,0.08400,0.08200,0.06200,0.04000,-0.00600,-0.04800,-0.08600,-0.10400,-0.12000,-0.10600,-0.09200,-0.06800,-0.03400,0.00200,0.03400,0.06400,0.07400,0.07800,0.07400,0.07000,0.06600,0.07200,0.07800,0.09200,0.10800,0.12200,0.12800,0.12800,0.12000,0.11000,0.09600,0.08600,0.07600,0.07200,0.07200,0.07600,0.08200,0.08800,0.08200,0.07400,0.02800,-0.02400,-0.06600,-0.09800,-0.13200,-0.12200,-0.10200,-0.08000,-0.04800,-0.01400,0.01400,0.03600,0.04600,0.04400,0.03800,0.03600,0.03400,0.04200,0.05800,0.08000,0.10200,0.12800,0.14600,0.15400,0.15000,0.14600,0.13200,0.11600,0.10600,0.10400,0.10200,0.10800,0.11600,0.12000,0.12200,0.11800,0.10800,0.06000,0.02000,-0.01400,-0.04200,-0.06200,-0.03800,-0.01400,0.00800,0.02600,0.03600,0.01000,-0.01600,-0.04200,-0.06800,-0.09000,-0.07800,-0.06200,-0.03400,0.00000,0.04000,0.07800,0.10400,0.11400,0.11400,0.10800,0.10000,0.08800,0.07600,0.07200,0.06800,0.06800,0.07400,0.08000,0.06200,0.05000,0.00800,-0.02800,-0.06000,-0.06600,-0.07400,-0.04200,-0.00800,0.02800,0.06400,0.09200,0.10800,0.11200,0.10000,0.09000,0.07800,0.06400,0.05800,0.06800,0.07400,0.09000,0.10800,0.12400,0.13200,0.13400,0.12200,0.10600,0.09000,0.07400,0.06000,0.06200,0.06600,0.07000,0.08000,0.08600,0.06000,0.04800,0.03600,-0.00400,-0.03400,-0.03000,-0.03200,-0.02800,0.01000,0.04400,0.07000,0.09000,0.10200,0.10200,0.08800,0.08200,0.06800,0.05200,0.04200,0.04800,0.05000,0.06000,0.07400,0.08000,0.07600,0.06200,0.02000,-0.01600,-0.04600,-0.06800,-0.08200,-0.06600,-0.05000,-0.02600,-0.00200,0.01600,0.00600,-0.00400,-0.02000,-0.04600,-0.06400,-0.05400,-0.04400,-0.02400,0.01000,0.04400,0.07600,0.09800,0.11000,0.11200,0.10600,0.10000,0.09800,0.09400,0.10000,0.11600,0.13400,0.15000,0.17000,0.18000,0.17400,0.16400,0.15400,0.12800,0.10800,0.09400,0.08000,0.07000,0.07000,0.06400,0.04600,0.01400,-0.02000,-0.05200,-0.08200,-0.09600,-0.09400,-0.09000,-0.08200,-0.06000,-0.02800,0.00400,0.03400,0.06000,0.07200,0.06800,0.06400,0.05400,0.04600,0.04600,0.05600,0.06800,0.08800,0.10800,0.12400,0.13400,0.13600,0.13400,0.09400,0.04800,0.01200,-0.01200,-0.03200,-0.01600,0.00400,0.00400,-0.01600,-0.03800,-0.06200,-0.08600,-0.10400,-0.11200,-0.12200,-0.12800,-0.12800,-0.11400,-0.08000,-0.03600,0.00800,0.05000,0.08400,0.09800,0.10200,0.10400,0.10400,0.10600,0.10600,0.10800,0.12400,0.13600,0.15000,0.16600,0.17400,0.17000,0.16000,0.14600,0.13400,0.12600,0.11800,0.11800,0.12000,0.12200,0.12400,0.11800,0.07400,0.02600,-0.02600,-0.07000,-0.10600,-0.10000,-0.08600,-0.05600,-0.02600,-0.02200,-0.00800,0.00400,-0.02000,-0.04400,-0.04200,-0.05400,-0.06800,-0.04800,-0.02400,0.00400,0.03800,0.07200,0.09600,0.10800,0.10800,0.09400,0.08200,0.06600,0.05200,0.04600,0.05200,0.05600,0.06600,0.07600,0.08200,0.08600,0.08400,0.04400,0.01000,-0.02600,-0.06400,-0.09400,-0.08200,-0.06800,-0.03800,-0.00400,0.02400,0.03000,0.03800,0.00600,-0.02400,-0.04600,-0.04200,-0.03800,0.00200,0.04200,0.08000,0.11400,0.13800,0.15200,0.16000,0.16000,0.15400,0.14600,0.13800,0.12800,0.12400,0.12200,0.12000,0.12200,0.11400,0.10600,0.06000,0.02000,-0.01400,-0.02800,-0.04000,-0.01000,0.02200,0.04800,0.06800,0.08800,0.10200,0.10600,0.07200,0.03200,-0.00600,-0.03800,-0.05800,-0.03600,-0.00200,0.02800,0.05400,0.07000,0.07800,0.07800,0.07200,0.03200,0.00000,-0.02400,-0.04600,-0.06000,-0.03400,-0.00800,0.01400,0.02800,0.01400,-0.00400,-0.03200,-0.06200,-0.08400,-0.08000,-0.07800,-0.06000,-0.02800,0.00800,0.04800,0.08600,0.11200,0.12200,0.12200,0.11400,0.10600,0.09600,0.09200,0.09200,0.10000,0.10800,0.12200,0.13000,0.13400,0.12000,0.11000,0.09400,0.05000,0.02000,0.01800,0.01400,0.01400,0.04600,0.07000,0.07800,0.08200,0.06200,0.05200,0.04200,0.02600,0.01600,0.03200,0.04000,0.04800,0.06600,0.07800,0.08600,0.09000,0.09000,0.08000,0.07400,0.06800,0.06400,0.06600,0.07800,0.09200,0.11000,0.12200,0.13200,0.13600,0.13000,0.12000,0.07600,0.03400,-0.00200,-0.02600,-0.04600,-0.02600,-0.01200,-0.02400,-0.03600,-0.05200,-0.08200,-0.10600,-0.10800,-0.11600,-0.12400,-0.11600,-0.10400,-0.07600,-0.03600,0.00800,0.05000,0.08400,0.09800,0.09800,0.09400,0.09000,0.08800,0.09200,0.10200,0.11800,0.13800,0.15800,0.17400,0.18600,0.18400,0.17600,0.15400,0.13600,0.11400,0.10000,0.08600,0.08400,0.08000,0.06800,0.06200,0.03400,-0.01000,-0.05000,-0.07600,-0.10800,-0.11400,-0.10200,-0.08600,-0.05800,-0.02200,0.01200,0.04000,0.06400,0.07000,0.07000,0.06400,0.06600,0.06600,0.07400,0.08400,0.10000,0.11400,0.12600,0.13200,0.13200,0.12200,0.11400,0.10200,0.08800,0.07800,0.07800,0.07600,0.08000,0.08600,0.09000,0.09000,0.08200,0.07200,0.02600,-0.02000,-0.05800,-0.09000,-0.11800,-0.10200,-0.07400,-0.04400,-0.01000,0.01800,0.03600,0.04600,0.04200,0.03400,0.03200,0.04000,0.04600,0.06600,0.09000,0.11400,0.13400,0.15200,0.15800,0.15800,0.15000,0.14000,0.12600,0.11600,0.10800,0.10800,0.11000,0.11600,0.12000,0.12000,0.11400,0.06800,0.03000,-0.00400,-0.03200,-0.05800,-0.04200,-0.02600,-0.00600,0.01400,0.03000,0.01400,-0.00600,-0.03200,-0.06400,-0.09000,-0.08800,-0.08400,-0.06800,-0.03600,0.00000,0.04000,0.07400,0.09800,0.10800,0.10800,0.09600,0.09000,0.07400,0.06400,0.06200,0.06800,0.06600,0.07600,0.07800,0.04600,0.02200,-0.01200,-0.04600,-0.07400,-0.06800,-0.06200,-0.03000,0.00400,0.04200,0.07800,0.10600,0.11600,0.11600,0.10200,0.08600,0.07200,0.06200,0.05800,0.06600,0.07800,0.09200,0.10800,0.12000,0.12600,0.12400,0.11200,0.10200,0.08200,0.07000,0.06400,0.06600,0.06600,0.07600,0.08400,0.08600,0.06200,0.05000,0.04000,-0.00200,-0.03400,-0.03200,-0.03600,-0.03000,0.01000,0.04600,0.07400,0.09400,0.10200,0.10200,0.09600,0.06800,0.05600,0.04200,0.03400,0.03400,0.05800,0.06800,0.07800,0.08600,0.08200,0.04600,0.01600,-0.02400,-0.06000,-0.08400,-0.07800,-0.07400,-0.04600,-0.01800,0.00400,0.00800,0.02000,0.01400,0.00200,-0.02800,-0.03400,-0.04800,-0.04400,-0.02800,0.01200,0.04800,0.07800,0.10000,0.10800,0.11000,0.10400,0.10200,0.09800,0.09600,0.10200,0.11600,0.13200,0.15200,0.17200,0.18400,0.18000,0.17400,0.14800,0.12600,0.10200,0.09000,0.07400,0.07200,0.07000,0.06800,0.03400,0.00600,-0.01800,-0.05800,-0.08800,-0.08800,-0.09200,-0.09600,-0.08200,-0.06000,-0.02600,0.00800,0.04000,0.06400,0.07400,0.06800,0.05000,0.04200,0.03600,0.03600,0.04600,0.07200,0.09000,0.11000,0.12600,0.13400,0.13200,0.12800,0.09200,0.07000,0.03800,0.02000,0.00200,0.01600,0.01200,0.00000,-0.01600,-0.04000,-0.06600,-0.09400,-0.10400,-0.12000,-0.12600,-0.13200,-0.12400,-0.10200,-0.06200,-0.01800,0.02800,0.06400,0.09000,0.09800,0.10000,0.10000,0.10200,0.10600,0.11400,0.12600,0.14000,0.15400,0.16800,0.17400,0.17600,0.17200,0.15800,0.14400,0.13400,0.12400,0.12000,0.12600,0.12800,0.12800,0.12600,0.12000,0.07000,0.01800,-0.02400,-0.06600,-0.10200,-0.09200,-0.07000,-0.04800,-0.02000,0.00200,-0.00600,-0.01400,-0.02600,-0.04200,-0.06000,-0.05000,-0.04400,-0.03000,-0.00600,0.02600,0.06000,0.09000,0.10600,0.11200,0.10200,0.09400,0.06600,0.05000,0.03600,0.03800,0.03600,0.05800,0.07000,0.08000,0.08400,0.08400,0.04400,0.00800,-0.03000,-0.06400,-0.09400,-0.08400,-0.07000,-0.04200,-0.01200,0.02000,0.04600,0.03200,0.01000,-0.01000,-0.03000,-0.04800,-0.02000,0.01200,0.04200,0.07400,0.11000,0.13400,0.15200,0.16400,0.16800,0.16000,0.15200,0.13800,0.12400,0.11800,0.12000,0.12000,0.12200,0.11800,0.11000,0.10000,0.05400,0.01800,-0.00200,-0.01600,-0.02200,0.01200,0.04200,0.06800,0.08600,0.09800,0.09600,0.09400,0.08200,0.03800,0.00000,-0.02000,-0.03400,-0.03400,0.00400,0.03800,0.06400,0.07600,0.07200,0.03600,0.00600,-0.02200,-0.04400,-0.06000,-0.04800,-0.03600,-0.01800,0.00200,0.02000,0.03800,0.01800,-0.00800,-0.03600,-0.06000,-0.09000,-0.08400,-0.07000,-0.04200,-0.00600,0.03800,0.07400,0.10600,0.12000,0.12600,0.12200,0.11800,0.10800,0.09800,0.09200,0.09600,0.10600,0.11600,0.13000,0.13600,0.13000,0.12200,0.07600,0.04400,0.02600,0.01600,0.00600,0.04000,0.06000,0.07200,0.08000,0.08600,0.08400,0.05600,0.03600,0.02400,0.01200,0.00600,0.03000,0.04800,0.06400,0.08000,0.09400,0.09800,0.06800,0.05600,0.04800,0.03600,0.03200,0.06200,0.07600,0.09000,0.11000,0.12600,0.13600,0.13400,0.13000,0.12000,0.07400,0.03600,0.00800,-0.01400,-0.03200,-0.02000};


float target_angle = 5.18; //摆平衡时的角度，手动测量，这个角度需要归一化，也就是0-2*PI，最好安装磁编码器的时候让这个值不要太接近0或者6.28,后边的代码没有处理边界情况
float bar_angle = 0;
float up_angle,costheta;
float angle_kp,angle_ki,angle_kd;
float threshold = 5;
float total_error=0;
float error=0;
float bar_vel,bar_vel_filtered;
float dead_force =0.0;// 0.05;
float base_vel,base_angle,base_vel_filtered;

float theta,theta_dot;
float last_force;

float abs_pos,off;

int state=1;
bool first_enter=true;
int i=0;


void setup() {
  //LQR其实可以通过建模直接得到控制参数，但是我没有建模，这里的参数还是手动调出来的
  angle_kp =-3;//-2.52;//-10;
  angle_kd =-0.1;//-0.15;// -150;
  angle_ki = 0.0053;

  ma730.clock_speed = 10000000; //SPI通讯频率
  // initialize encoder sensor hardware
  encoder.init();
  encoder.enableInterrupts(doA, doB); 
  // link the motor to the sensor
  motor.linkSensor(&encoder);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  // aligning voltage [V]
  motor.voltage_sensor_align = 3;

  // set motion control loop to be used
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  // contoller configuration 
  // default parameters in defaults.h

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 10;
  motor.PID_velocity.D = 0;
  // default voltage_power_supply
  motor.voltage_limit = 6;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
  
  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01;

  // use monitoring with serial 
  Serial.begin(2000000);
  // comment out if not needed
  motor.useMonitoring(Serial);

  // initialize motor
  motor.sensor_direction=Direction::CW;
  motor.init();
  ma730.init();
  // align sensor and start FOC
  motor.initFOC();



  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target velocity using serial terminal:"));
  Serial.println(F_CPU);
  _delay(500);
  off=motor.zero_electric_angle/7.0;
  //取一下前面的数据，不然滤波可能有问题
  for(int j=0;j<1000;j++){
    bar_angle = ma730.getAngle();
    bar_vel=ma730.getVelocity();
    base_angle=encoder.getAngle();
    base_vel = encoder.getVelocity();
    bar_vel_filtered=filter(bar_vel);
    base_vel_filtered=base_filter(base_vel); 
  }
  //配置定时器中断 开始想让FOC控制和计算放在定时器中断里，写着写着嫌麻烦就不用中断了
//  TIM_TypeDef *Instance = TIM1;
//  HardwareTimer *MyTim = new HardwareTimer(Instance);
//
//  MyTim->setOverflow(2000, MICROSEC_FORMAT); // 1000 Hz
//  MyTim->attachInterrupt(Update_IT_callback);
//  MyTim->resume();

}


void loop() {
 //对于摆来说，磁钢在安装的时候可能方向不一样，所以摆的角度的正方向不确定，需要调整角度和速度的方向为逆时针(正视图)
 //对于基座来说，由于光电编码器的AB两线和单片机的连接顺序不定，基座的正方向也不确定，需要调整角度和速度的方向为逆时针(俯视图)
 //需要根据正方向自行调节下面的代码
  bar_angle = ma730.getAngle();
  bar_vel = ma730.getVelocity();
  base_vel = encoder.getVelocity();
  
  bar_vel_filtered=filter(bar_vel);
  base_vel_filtered=base_filter(base_vel);
  //abs_pos=-off+encoder.getAngle(); //校准时候用的，先不用
  error=_normalizeAngle(bar_angle)-target_angle;


  theta=3.1415+error; //theta是定义为摆的角度，摆竖直向下的时候为0，逆时针为正
  theta_dot=bar_vel_filtered;
  costheta=cos(theta); //提前计算，方便后边使用
  //一个小状态机，一共四个状态
  switch(state){
    case 1: //idle 空状态，当摆竖直朝下的时候，给一个脉冲，不然energy shaping法无法启动，摆不能自己摆起来
      if(i<100){
        _delay(1);
        target_force=0.8; 
        i++;
      }
      else
        state=2;
      break;
    case 2: //swing up 使用energy shaping法摆起来，接近平衡位置的时候切换状态到LQR
      target_force = 0.003*theta_dot*costheta*(-8.5*(1+costheta)+0.0075*theta_dot*theta_dot); 
      if(abs(error)<0.5)
        state=3;
      break;
    case 3: //LQR 这里的参数是手动调的，结构上使用的是LQR的结构
      target_force =base_vel_filtered*0.1-1.8*error-0.08*bar_vel_filtered;
      if(abs(error)>0.6)
        state=4;
      break;
    case 4: //swing down 就是Swing up的逆向使用，为了更快的让摆竖直向下
      target_force = -0.005*theta_dot*costheta*costheta*costheta*costheta*(-8.5*(1+costheta)+0.0075*theta_dot*theta_dot);
      if(abs(theta)<0.1&&abs(theta_dot)<0.1){
        state=1;
        i=0;
      }
        
      break;
  }
//如果力矩方向和基座方向不一致，需要取反
//target_force=-target_force;

  //死区补偿
  if(target_force>threshold)
    target_force=threshold;
  else if(target_force<-threshold)
    target_force=-threshold;

//下面都是串口调试
  Serial.print(state); Serial.print('\t');
  Serial.print(error); Serial.print('\t');
  Serial.print(theta); Serial.print('\t');
  Serial.print(bar_vel_filtered); Serial.print('\t');
  Serial.println(base_vel_filtered);
//Serial.print(_normalizeAngle(abs_pos),5);
//  Serial.println(target_force,5);
//  Serial.print('\t');
////  Serial.print((int)(_normalizeAngle(abs_pos)/0.00628));
//  Serial.print('\t');
//  Serial.println(up_angle,5);
//    Serial.println(target_angle,5);
//  Serial.print(bar_angle,5);
//  Serial.print('\t');
//  Serial.print(bar_vel_filtered,5);
//  Serial.print('\t');
//  Serial.print(base_angle,5);
//  Serial.print('\t');
//  Serial.println(base_vel_filtered,5);
//    Serial.print(error);
//    Serial.print('\t');
//    Serial.print(base_vel,5);
//    Serial.print('\t');
//    Serial.println(bar_vel,5);
//    Serial.println(total_error);
//    Serial.print(bar_vel,3);
//    Serial.print('\t');
//  Serial.println(angle_kd*bar_vel,4);
//  Serial.print('\t');
//  Serial.println(target_force);

  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz 
  motor.loopFOC();

  // Motion control function
  // velocity, position or voltage (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the motor.target in the code

  //注释掉了校准力矩
  //target_force+=calib[(int)(_normalizeAngle(abs_pos)/0.00628)];  
  if(target_force>0)target_force+=dead_force;
  else if(target_force<0)target_force-=dead_force;
  //转速过大就保护
  if(abs(base_vel_filtered)>40) target_force=0;
  motor.move(target_force);


}
